# ULTRASOUND

import sys
import serial
import time
import wx
from threading import Thread

PORTNAME = 'COM1'

if len(sys.argv) < 2:
    print 'you can select port with first argument eg: COM3'
else:
    PORTNAME = sys.argv[1]
    print 'chosen port: %s' % (PORTNAME, )
    
class MyFrame(wx.Frame):
    
    def __init__(self, parent, title):
        wx.Frame.__init__(self, parent, -1, title, pos=(150, 150), size=(500, 500))        
        
        areaD = {}
        areaD['b'] = 'b'
        areaD['l'] = 'l'
        areaD['d'] = 'd'         
        areaList = sorted(areaD.keys())
        
        areaD = {}
        areaD['FORWARD'] = 1
        areaD['BACKWARD'] = -1
        areaList2 = areaD.keys()
        
        panel = wx.Panel(self)

        lbl_x = wx.StaticText(panel, -1, "lbl_x")
        self.txt_x = wx.TextCtrl(panel, -1, "")
        lbl_y = wx.StaticText(panel, -1, "lbl_y")
        self.txt_y = wx.TextCtrl(panel, -1, "")
        lbl_r1_corrected = wx.StaticText(panel, -1, "Hello World!")
        self.txt_r1_corrected = wx.TextCtrl(panel, -1, "")
        lbl_r2_corrected = wx.StaticText(panel, -1, "Hello World!")
        self.txt_r2_corrected = wx.TextCtrl(panel, -1, "")
        lbl_r1_corrected_2 = wx.StaticText(panel, -1, "Hello World!")
        self.txt_r1_corrected_2 = wx.TextCtrl(panel, -1, "")
        lbl_r2_corrected_2 = wx.StaticText(panel, -1, "Hello World!")
        self.txt_r2_corrected_2 = wx.TextCtrl(panel, -1, "")
        
        
        self.btn_start = wx.Button(panel, -1, "Start")
        self.btn_stop = wx.Button(panel, -1, "STOP")
        self.btn_speed_test = wx.Button(panel, -1, "Speed Test")
        
        self.Bind(wx.EVT_BUTTON, self.FncStart, self.btn_start)
        self.Bind(wx.EVT_BUTTON, self.FncStop, self.btn_stop)
        self.Bind(wx.EVT_BUTTON, self.FncSpeedTest, self.btn_speed_test)

        self.speed_test_motors_combo = wx.ComboBox(panel, -1, value=areaList[0], choices=areaList)
        self.speed_test_direction_combo = wx.ComboBox(panel, -1, value=areaList2[0], choices=areaList2)
        
        siz = wx.BoxSizer(wx.HORIZONTAL)

        sizer = wx.BoxSizer(wx.VERTICAL)
        sizer.Add(lbl_x, 0, wx.ALL, 10)
        sizer.Add(lbl_y, 0, wx.ALL, 10)
        sizer.Add(lbl_r1_corrected, 0, wx.ALL, 10)
        sizer.Add(lbl_r2_corrected, 0, wx.ALL, 10)
        sizer.Add(lbl_r1_corrected_2, 0, wx.ALL, 10)
        sizer.Add(lbl_r2_corrected_2, 0, wx.ALL, 10)        
        sizer.Add(self.btn_start, 0, wx.ALL, 10)
        sizer.Add(self.btn_stop, 0, wx.ALL, 10)
        sizer.Add(self.btn_speed_test, 0, wx.ALL, 10)
        sizer.Add(self.speed_test_motors_combo, 0, wx.ALL, 10)
        sizer.Add(self.speed_test_direction_combo, 0, wx.ALL, 10)
        
        sizer2 = wx.BoxSizer(wx.VERTICAL)
        sizer2.Add(self.txt_x, 0, wx.ALL, 5)
        sizer2.Add(self.txt_y, 0, wx.ALL, 5)
        sizer2.Add(self.txt_r1_corrected, 0, wx.ALL, 5)
        sizer2.Add(self.txt_r2_corrected, 0, wx.ALL, 5)
        sizer2.Add(self.txt_r1_corrected_2, 0, wx.ALL, 5)
        sizer2.Add(self.txt_r2_corrected_2, 0, wx.ALL, 5)
        
        siz.Add(sizer,0,wx.ALL,0)
        siz.Add(sizer2,0,wx.ALL,0)
                
        panel.SetSizer(siz)
        panel.Layout()


    def FncStart(self, evt):
        print "START!"
        if not self.handler:
            self.handler = PortISR(port)
            self.handler.start()
        if not self.handler.port.isOpen():
            self.handler.port.open()
        self.handler.default_engine_start()
        

    def FncStop(self, evt):
        print "STOP!"
        if self.handler:
            if self.handler.port.isOpen():
                self.handler.stop_engines()
                
        
    def FncSpeedTest(self, evt):
        print "SPEED TEST!"
        if self.handler:
            if self.handler.port.isOpen():
                self.handler.speed_test(self.speed_test_motors_combo.GetValue(), self.speed_test_direction_combo.GetValue())

class Packet:
    """ defines format of data received from device
    """
    
    LENGTH = 9
    STARTER = '@'
    
    def __init__(self, x=0, y=0, r1=0, r2=0, r1_corrected=0, r2_corrected=0,r1_corrected_2=0,r2_corrected_2=0):
        self.x = x
        self.y = y
        self.r1 = r1
        self.r2 = r2
        self.r1_corrected = r1_corrected
        self.r2_corrected = r2_corrected
        self.r1_corrected_2 = r1_corrected_2
        self.r2_corrected_2 = r2_corrected_2

    def load(self, chunk):
        """ loads from chunk, eats up chunk.
        """
        while len(chunk) >= self.LENGTH:
            if chunk[0] == self.STARTER:
                self.x = self.packit(chunk[1], chunk[2])
                self.y = self.packit(chunk[3], chunk[4])
                self.r1 = self.packit(chunk[5], chunk[6])
                self.r2 = self.packit(chunk[7], chunk[8])
                for x in range(self.LENGTH): chunk.pop(0)
                return True
            else:
                chunk.pop(0)
        return False

    def packit(self, hi, lo):
        ret = ((int(hi))<<8) | (int(lo))
        if hi & 0x80: ret |= 0xFFFF0000
        return ret

class PortISR(Thread):
    """ handles onreceive port event ..
    """
    
    FORWARD = 1
    BACKWARD = -1
    
    def __init__(self, left_speed, right_speed, stop_speed):
        Thread.__init__(self)
        self._port = port
        self._packet = Packet()
        self.chunk = ''
        self.left_speed=left_speed
        self.right_speed=right_speed
        self.stop_speed=stop_speed
        self.current_direction = self.FORWARD
        self.previous_x = 0
        
        print 'open %s' % (PORTNAME, )
        if not self.port.isOpen():
            self.port = serial.Serial(PORTNAME)
            self.port.open()
    
    def run(self):
        while 1:
            self.chunk += self._port.read()
            if self._packet.load( self.chunk ):
                
                self._packet.r1_korigovano_2 = self._packet.r1 * 3.2/2.9 - 281;
                self._packet.r2_korigovano_2 = self._packet.r2 * 3.2/2.9 - 281;
                self._packet.r1_korigovano = (self._packet.r1 * 343 * 3.2)/1000000.0;
                self._packet.r2_korigovano = (self._packet.r2 * 343 * 3.2)/1000000.0;
                if self.current_direction == self.FORWARD:
                    if self._packet.x < self.previous_x:
                        if self._packet.x>250 and self._packet.x<300:
                            walking('d',self.right_speed,self.BACKWARD)
                            walking('l',self.left_speed,self.BACKWARD)
                    #else:       
                elif self._packet.x > self.previous_x:
                    if self._packet.x>1000 and self._packet.x<1050:
                        walking('d',self.right_speed,self.FORWARD);                            
                        walking('l',self.left_speed,self.FORWARD);
                    #else:
                self.previous_x = self._packet.x
    
    def walking(self,motor,speed,direction):
        brz = speed_calculation(motor,speed,direction)
        self.port.write(brz)
        
    def speed_test_walking(self,motor,speed,direction):
        self.port.write(speed*direction)
    
    def stop_engines(self):
        walking('d',self.stop_speed,self.current_direction)
        walking('l',self.stop_speed,self.current_direction)
        
    def default_engine_start(self):
         walking('d',self.right_speed,self.current_direction)
         walking('l',self.left_speed,self.current_direction)
    
    def speed_test(self,which_motor,direction):
        if direction == 'FORWARD':
            direction = 1
        elif direction == 'BACKWARD':
            direction = -1
            
        if which_motor == 'l':
            for i in range(0,64):                 
                 time.sleep(0.01)
                 speed_test_walking('l',i+192,direction)
            for i in range(64,128):
                 time.sleep(0.01)
                 speed_test_walking('l',i+64,direction)
        elif which_motor == 'd':
                for i in range(0,64):
                     time.sleep(0.01)
                     speed_test_walking('d',i,direction)
                for i in range(64,128):
                     time.sleep(0.01)
                     speed_test_walking('d',i,direction)
        else:
            for i in range(0,64):
                 time.sleep(0.01)
                 speed_test_walking('l',i+192,direction)
                 speed_test_walking('d',i,direction)
            for i in range(64,128):
                 time.sleep(0.01)
                 speed_test_walking('l',i+64,direction)
                 speed_test_walking('d',i,direction)
                
    
    def speed_calculation(self,motor,speed,direction):
        
        self.current_direction = direction;

        speed = speed * direction;

        if motor == 'l' or motor=='L':
            if brzina<0:
                brzina=brzina*(-1);
                brzina = brzina + 128;
            else:
                brzina = brzina + 192;
        else:
            if motor == 'd' or motor == 'D':
                if brzina<0:
                    brzina=brzina*(-1);
                    brzina = brzina + 64;                                    
        return brzina;

class MyApp(wx.App):
    def OnInit(self):
        frame = MyFrame(None, "Simple wxPython App")
        self.SetTopWindow(frame)

        print "Print statements go to this stdout window by default."

        frame.Show(True)
        return True
        
app = MyApp(redirect=True)
app.MainLoop()
